import math
import socket
import json
import time
# Author:chenliao@elibot.cn

def connectETController(ip, port=8055):
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    try:
        sock.connect((ip, port))
        return (True, sock)
    except Exception as e:
        sock.close()
        return (False,)

def disconnectETController(sock):
    if (sock):
        sock.close()
        sock = None
    else:
        sock = None

def sendCMD(sock, cmd, params=None, id=1):
    if (not params):
        params = []
    else:
        params = json.dumps(params)
    sendStr = "{{\"method\":\"{0}\",\"params\":{1},\"jsonrpc\":\"2.0\",\"id\":{2}}}".format(
        cmd, params, id) + "\n"
    try:
        # print(sendStr)
        sock.sendall(bytes(sendStr, "utf-8"))
        ret = sock.recv(1024)
        jdata = json.loads(str(ret, "utf-8"))
        if ("result" in jdata.keys()):
            return (True, json.loads(jdata["result"]), jdata["id"])
        elif ("error" in jdata.keys()):
            print(sock.error["code"])
            print(sock.error["message"])
            return (False, json.loads(jdata["error"]), jdata["id"])
        else:
            return (False, None, None)
    except Exception as e:
        return (False, None, None)

# 等待机器人完成动作
def wait_stop():
    while True:
        time.sleep(0.01)
        ret1, result1, id1 = sendCMD(sock, "getRobotState")
        # print(ret1,result1)
        if (ret1):
            if result1 == 0 or result1 == 4:
                break
        else:
            print("getRobotState failed")
            break

def Offs(p, x, y, z):
    # 偏移函数
    p1 = p.copy()
    p1[0] = p1[0]+x
    p1[1] = p1[1]+y
    p1[2] = p1[2]+z
    return p1

if __name__ == "__main__":
    robot_ip = "192.168.1.233"
    conSuc, sock = connectETController(robot_ip)

    # 如果机器人有报警，清除报警
    ret1, result1, id1 = sendCMD(sock, "getRobotState")
    if result1 == 4:
        ret, result, id = sendCMD(sock, "clearAlarm")
        print(ret, result)

    # 获取同步状态
    suc, Motorstatus, id = sendCMD(sock, "getMotorStatus")
    if Motorstatus == 0:
        # 同步伺服编码器数据
        suc, syncMotorStatus, id = sendCMD(sock, "syncMotorStatus")

    # 设置伺服使能状态
    suc, servostatus, id = sendCMD(sock, "set_servo_status", {"status": 1})

    # 设置机器人开始位置，类型joint
    P000 = [-87.754, -91.429, 91.070, -89.539, 91.151, -4.523]
    # MoveJ形式移动机器人到P001位置，速度为百分比
    suc, result, id = sendCMD(sock, 'moveByJoint', {
                              'targetPos': P000, 'speed': 10})
    # 等待机器人移动开始位置
    wait_stop()

    # 获取开始位置的笛卡尔坐标
    ret1, startpose, id = sendCMD(sock, "get_tcp_pose")
    print('startpose', startpose)

    # 通过Offs函数，计算得到新的目标点，并通过inverseKinematic计算得到对应的逆解（joint）
    suc, P1 , id=sendCMD(sock,"inverseKinematic",{"targetPose": Offs(startpose,50,0,0)})
    suc, P2 , id=sendCMD(sock,"inverseKinematic",{"targetPose": Offs(startpose,50,50,0)})
    suc, P3 , id=sendCMD(sock,"inverseKinematic",{"targetPose": Offs(startpose,0,50,0)})
    suc, P4 , id=sendCMD(sock,"inverseKinematic",{"targetPose": Offs(startpose,0,0,0)})

    # 使用moveByLine/moveByJoint指令时，不能设置点与点之间的圆滑，机器人会精确走到每个点
    suc, result, id = sendCMD(sock, 'moveByLine', {'targetPos': P1, "speed_type": 0, "speed": 30})
    wait_stop()
    suc, result, id = sendCMD(sock, 'moveByLine', {'targetPos': P2, "speed_type": 0, "speed": 30})
    wait_stop()
    suc, result, id = sendCMD(sock, 'moveByLine', {'targetPos': P3, "speed_type": 0, "speed": 30})
    wait_stop()
    suc, result, id = sendCMD(sock, 'moveByLine', {'targetPos': P4, "speed_type": 0, "speed": 30})
    wait_stop()
  
    # 先将之前的轨迹点位清除
    suc, result , id = sendCMD(sock, "clearPathPoint")
    # 使用运动2.0指令，使用addPathPoint添加点位，可以设置每一句的运动模式和转弯半径等参数，达到轨迹圆滑效果
    # 先将需要运动的所有点（joint类型）发送给机器人。
    suc, result , id = sendCMD(sock, "addPathPoint", {"wayPoint": P1,"moveType": 1, "speed":30, "circular_radius":20})
    suc, result , id = sendCMD(sock, "addPathPoint", {"wayPoint": P2,"moveType": 1, "speed":30, "circular_radius":20})
    suc, result , id = sendCMD(sock, "addPathPoint", {"wayPoint": P3,"moveType": 1, "speed":30, "circular_radius":20})
    suc, result , id = sendCMD(sock, "addPathPoint", {"wayPoint": P4,"moveType": 1, "speed":30, "circular_radius":20})
    # 开始移动
    suc, result , id = sendCMD(sock, "moveByPath")
    wait_stop()